#include "robothelper.h"
using namespace std ;
using namespace Eigen;

#include <QDebug>
#include <QTimer>
#include <boost/scope_exit.hpp>


static Config _robotConf("etc/communication/robot.sys");

RobotHelper::RobotHelper()
{
    //recodeBug1();
    // SelectRobot();
}



void RobotHelper::InstallOutlineEvent()
{
    _allowMove = false;     /// 禁止移动
}

void RobotHelper::ReConnect()
{
    initMemberData();       /// 初始化成员
}

void RobotHelper::PauseMotion(bool state)
{
//    if(state)
//        // _robot->stopMove(RobotError::FORCESTOPMOVING) ;
//    else{
//        ///
//    }
}

void RobotHelper::CriticalStop(bool state)
{
//    if(state)
//        // _robot->stopMove(SERVICESTOP) ;
//    else{
//        ///
//    }
}

void RobotHelper::RelativeMove(RobotPos pos,int speed )
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    try{
        std::future<MU_RETURNS> result =  std::async(std::launch::async,&RobotHelper::_relativeMove,this, pos,speed);
        if(MU_RETURN_SUCCESS != result.get()){
            qWarning()<<"相对运动失败!"<<__FUNCTION__;
        };
    }catch(MU_EXPS::ROBOT_EXPECTION e){
        std::cout<< BOLDRED<<GetErrString(MU_Messages::Robot,e)<<__FUNCTION__<< RESET <<std::endl;
    }
}

void RobotHelper::ABSMove(RobotPos target, int speed)
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    if(target == RobotPos::instance("0,0,0,0,0,0")){
        pushWarn("RobotPos信息为空!",MU_ERROR_MODULE_ROBOT);
        return;
    }
//    /**
//     * @brief 1
//     * @details 创建一个异步非阻塞线程，并在其结束后销毁，缺点:无法获取当前的执行状态
//     */
//    std::thread ([this,&target,&speed](){
//        try{
//            _absMove(target,speed);
//        }catch(MU_EXPS::ROBOT_EXPECTION e){
//            std::cout<< BOLDRED<<GetErrString(MU_Messages::Robot,e)<<__FUNCTION__<< RESET<<std::endl ;
//        }
//    }).detach();

    std::cout<<target.toStr() <<std::endl;
    //
     _absMove(target,speed);

    /**
     * @brief rt1
     * @warning 同步阻塞式携程
     */
//    coroutine::routine_t rt1 = coroutine::create(std::bind(&RobotHelper::_absMove,this, target,speed));
//    coroutine::resume(rt1);
//    coroutine::destroy(rt1);

    /**
     * @brief coroutine::await
     * @details 异步的执行任务，但是会阻塞
     */
    // coroutine::await(std::bind(&RobotHelper::_absMove,this, target,speed));
    return;
}

std::future<MU_RETURN_LIST> RobotHelper::ABSMove(std::vector<RobotPos> target, int speed)
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;

    std::future<MU_RETURN_LIST> result;
    try{
        std::thread th2(std::bind(&RobotHelper::_absMoveC,this, target,speed), std::ref(result));
        th2.detach();
    }catch(MU_EXPS::ROBOT_EXPECTION e){
        std::cout<< BOLDRED<<GetErrString(MU_Messages::Robot,e)<<__FUNCTION__<< RESET<<std::endl ;
    }

    return result;
}

void RobotHelper::ABSMoveAxis(RobotAxle target, int speed)
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    if(target == RobotAxle::instance("0,0,0,0,0,0")){
        pushWarn("轴信息为空!",MU_ERROR_MODULE_ROBOT);
    }
    try{
        std::future<void> result =  std::async(std::launch::async,&RobotHelper::_absMoveAxis,this, target,speed);
        result.get();
    }catch(MU_EXPS::ROBOT_EXPECTION e){
        std::cout<< BOLDRED<<GetErrString(MU_Messages::Robot,e)<<__FUNCTION__<< RESET<<std::endl ;
    }
}


void RobotHelper::OutSilk()
{

}

void RobotHelper::InSilk()
{

}

void RobotHelper::ConnectToRobot()
{
    /// 方法1
//    std::packaged_task<MU_RETURNS(void)> task(&RobotHelper::_connect) ;
//    std::future<MU_RETURNS> result = task.get_future();
//    std::thread(this,std::move(task)).detach();

    if(currentStatus.GetInit(MU_DEVICE_ROBOT)){     /// 如果已经初始化了
        qWarning()<<"机器人已经初始化成功!"<<__FUNCTION__;
        return ;
    }
    currentStatus.setInitType(MU_DEVICE_ROBOT,MU_TY_RUNNING);
    std::future<MU_RETURNS> result = std::async(std::launch::async,&RobotHelper::_connect,this);


    if(MU_RETURN_SUCCESS != result.get()){
        currentStatus.initFailed(MU_DEVICE_ROBOT);
        std::cout << "机器人连接:" << MU_Get_Msg(result.get()) << ".\n";
        return ;
    }

    currentStatus.initSucess(MU_DEVICE_ROBOT);
}


void RobotHelper::GetError()
{
//    // 获取机器人故障状态
//    if(_robot->rgsoStatus < 0)
//    {
//        cout<<"rgso Status < 0!"<<endl;
}

void RobotHelper::CloseRobot()
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    _allowMove = false;
    UDPHelper::GetInstance()->p()->DisConnectToHost();
    UDPHelper::GetInstance()->p()->WriteBuff("stop");
}

void RobotHelper::handlerDis()
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    currentStatus.initFailed(MU_DEVICE_CAM);
    TaskManager::GetInstance()->setATask(MU_TASK_INDEX_CAM,false);
    CloseRobot();
    pushExp(MU_Messages::Robot,MU_EXPS::EXP_RBT_DIS,MU_ERROR_MODULE_ROBOT,DateTime::getFullTime());
}

void RobotHelper::OpenWeld(weldInfo r)
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    threshold = 0;  /// 焊接的时候，阈值为0,出现任何问题都应该停止焊接
    _robot->setWeldingTrue( r);
}

void RobotHelper::CloseWeld()
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    threshold = 5;  /// 不焊接的时候，阈值为5
    _robot->setWeldingFalse();
}

void RobotHelper::MoveToSafePos()
{
    ABSMove(defaultPos);
    ABSMoveAxis(defaultAxle);
}

void RobotHelper::RetractMove(RobotPos s, int len, int dy, int dx)
{
    retractMove(s,len,dy,dx);
    ABSMove(s);
}

bool RobotHelper::GetConEnd()
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    return _threadHandle.empty();
}

RobotPos RobotHelper::getCurrentPos(){
    return _robot->getCurrPos(RobotTCP::UPLOAD);
}


void RobotHelper::ProcessContinousPos(std::vector<RobotPos> target, int speed)
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    try {
        if(1 < abs(_sys_config.get<double>("robot.speed")-speed)){
            //_sys_config.put("robot.speed", speed);
            _sys_config.sync();
        }
    }catch( const boost::exception &e ) {
        DUMPERROR(e) ;
        qWarning()<< BOLDRED<<"Robot配置文件操作失败!"<<__FUNCTION__<< RESET ;
        throw MU_EXPS::EXP_RBT_SYSCONF;
    }

    for(auto &d:target){
        _threadHandle.push_back(d);
    }
}

void RobotHelper::StartUDP()
{
    try {
        UDPHelper::GetInstance()->p()->ConnectToHost();
    } catch (...) {
        throw MU_EXPS::EXP_RBT_COM;
    }

}

void RobotHelper::ReadConfig()
{
    try {
        switch ((ROBOT::Tag_COM_TYPE)_robotConf.get<int>("comType")) {
        case ROBOT::T_UDP:
            UDPHelper::GetInstance()->p()->SetIP(_robotConf.get<string>("UDP.ip").c_str(),_robotConf.get<int>("UDP.port"));
            break;
        case ROBOT::T_TCP:
            UDPHelper::GetInstance()->p()->SetIP(_robotConf.get<string>("TCP.ip").c_str(),_robotConf.get<int>("TCP.port"));
            break;
        default:
            break;
        }
    } catch (...) {
        /// todo
        throw MU_EXPS::EXP_RBT_SYSCONF;
    }
}

void RobotHelper::initMemberData()
{
     _allowMove = true;

     std::thread *back = new std::thread([this](){
         RobotPos nullPos = RobotPos::instance();
         weldInfo nu_ptr = {0,0,0,0,0,0,0,0,0};
         int  goIndex = 0;
         while (getSystemStatus()) {
             bool ride = ((currentStatus.expLock.load() < threshold) );
             if(!ride){ // 如果到达阈值上限，则无论如何都要关闭焊接
                    RobotHelper::GetInstance()->CloseWeld();
             }
             while ( _allowMove && ride) {
                 RobotPos temp = RobotPos::instance();
                 while (TaskManager::GetInstance()->getProcess(MU_TASK_INDEX_ROBOT)) { // 如果当前在暂停态,则跳过本次循环
                     sleep_s(1);
                     break;
                 }
                 while (!zeroLock.load());//当队列插入过了才运行
                 if(_threadHandle.empty()){
                     goIndex = 0;
                     continue;
                 }

                 auto cur = _robot->getCurrPos( RobotTCP::UPLOAD );
                 auto &_ptr0 =  _threadHandle.at(0);


                 try {
                     // _robot->multMove(goIndex,_ptr0+cur);
                 }catch(ReturnStatus e){
                     if(e == ERR_ROBOT_MOVEING_EXCEPTION || e == ERR_ROBOT_POS_NOT_SAFE){
                         /// todo
                         /// 此处写入多点运动失败
                         pushExp(MU_Messages::Robot,MU_EXPS::EXP_RBT_MOVE,MU_ERROR_MODULE_ROBOT,DateTime::getFullTime());
                     }
                 }

                 _threadHandle.pop_front();
                 goIndex++;
             }
        }
     });

     ThreadPool::GetInstance()->pushPermanentAsync(MU_THREAD_CONTINOUS,back);





        /// 更新当前Pos及轴信息
//    std::thread mythread([](){
//        int a =0;
//        a = 5 + 0;
//    },this);
}

MU_RETURNS RobotHelper::_connect()
{
    std::lock_guard<recursive_mutex> lck(_mtx) ;
    try {
       SelectRobot();
       ReadConfig();
       StartUDP();
       /// 2.启动机器人事件循环
       UDPHelper::GetInstance()->p()->WriteBuff("start");



       initMemberData();    /// 初始化后台线程
       ///
       std::this_thread::sleep_for( std::chrono::milliseconds(500) ) ;
       GetError();
    } catch (MU_EXPS::ROBOT_EXPECTION e) {
        pushExp(MU_Messages::Robot,e,MU_ERROR_MODULE_ROBOT,DateTime::getFullTime());
        return  MU_RETURN_ERROR;
    }

    return  MU_RETURN_SUCCESS;

}

MU_RETURNS RobotHelper::_relativeMove(RobotPos pos,int speed)
{
    RobotPos relativePos = pos;
    try{
        _robot->setSpeed(speed);
        _robot->setSpeed(speed);

        if ( !_sys_config.get<bool>("robot.bEular")) {
            _robot->relativeMove_axis( {relativePos},
                                       false) ;
        }
        else {
            _robot->relativeMove( relativePos, false ) ;
        }

    }catch( const boost::exception &e ) {
        DUMPERROR(e);
        throw MU_EXPS::EXP_RBT_SYSCONF;
        return MU_RETURN_ERROR;
    }catch(ReturnStatus e){
        qWarning()<< BOLDRED<<"基础库有异常触发!"<<__FUNCTION__<< RESET ;
        if(e == ERR_ROBOT_MOVEING_EXCEPTION || e == ERR_ROBOT_POS_NOT_SAFE){
            throw MU_EXPS::EXP_RBT_MOVE;
            return MU_RETURN_ERROR;
        }
    }
    return MU_RETURN_SUCCESS;
}

void RobotHelper::_absMove(RobotPos target, int speed)
{
    try{
        _robot->setSpeed(speed);
        _robot->setSpeed(speed);
        _robot->absoluteMove(target);
    }catch( const boost::exception &e ) {
        DUMPERROR(e) ;
        qWarning()<< BOLDRED<<"Robot配置文件操作失败!"<<__FUNCTION__<< RESET ;
        throw MU_EXPS::EXP_RBT_SYSCONF;
    }catch(ReturnStatus e){
        qWarning()<< BOLDRED<<"基础库有异常触发!"<<__FUNCTION__<< RESET ;
        if(e == ERR_ROBOT_MOVEING_EXCEPTION || e == ERR_ROBOT_POS_NOT_SAFE){
            throw MU_EXPS::EXP_RBT_MOVE;
        }
    }
}

MU_RETURNS RobotHelper::_absMoveC(std::vector<RobotPos> target, int speed)
{
    try{
        _robot->setSpeed(speed);
        _robot->setSpeed(speed);
        _robot->moveAlongPath(target);
        return MU_RETURN_SUCCESS;
    }catch( const boost::exception &e ) {
        DUMPERROR(e) ;
        qWarning()<< BOLDRED<<"Robot配置文件操作失败!"<<__FUNCTION__<< RESET ;
        throw MU_EXPS::EXP_RBT_SYSCONF;
    }catch(ReturnStatus e){
        qWarning()<< BOLDRED<<"基础库有异常触发!"<<__FUNCTION__<< RESET ;
        if(e == ERR_ROBOT_MOVEING_EXCEPTION || e == ERR_ROBOT_POS_NOT_SAFE){
            throw MU_EXPS::EXP_RBT_MOVE;
        }
    }
    return MU_RETURN_ERROR;
}

void RobotHelper::_absMoveAxis(RobotAxle target, int speed)
{
    try{
        _robot->setSpeed(speed);
        _robot->setSpeed(speed);
        _robot->absoluteMove(target);
    }catch( const boost::exception &e ) {
        DUMPERROR(e) ;
        qWarning()<< BOLDRED<<"Robot配置文件操作失败!"<<__FUNCTION__<< RESET ;
        throw MU_EXPS::EXP_RBT_SYSCONF;
    }catch(ReturnStatus e){
        qWarning()<< BOLDRED<<"基础库有异常触发!"<<__FUNCTION__<< RESET ;
        if(e == ERR_ROBOT_MOVEING_EXCEPTION || e == ERR_ROBOT_POS_NOT_SAFE){
            throw MU_EXPS::EXP_RBT_MOVE;
        }
    }
}
